package edu.wpi.first.wpilibj.templates.commands;
import edu.wpi.first.wpilibj.DriverStationLCD;

public class DoDriveDistance extends CommandBase {
    double Speed;
    double Distance; //distance is in feet

    public DoDriveDistance(double s, double d) {
        //Declares the input for the function
        requires(MyDriveTrain);
        this.Speed=s;
        this.Distance=d;
    }

    protected void initialize() {
        MyDriveTrain.ResetEncoders();
        MyDriveTrain.StartEncoders();
    }

    protected void execute() {
        //Declares what drive you need to use and the speed of it
        MyDriveTrain.TankDrive(Speed,Speed);
        //Prints out right and left encoder values on dashboard
        DriverStationLCD lcd = DriverStationLCD.getInstance();
        DriverStationLCD.getInstance().println(
                DriverStationLCD.Line.kUser2, 1,
                "LEncoder: " + MyDriveTrain.GetLeftEncoderValue());
        DriverStationLCD.getInstance().println(
                DriverStationLCD.Line.kUser3, 1,
                "REncoder: " + MyDriveTrain.GetRightEncoderValue());
        lcd.updateLCD();
    }

    protected boolean isFinished() {
        //Declares when to stop encoders by averaging their distance 
        if(Distance-((MyDriveTrain.GetLeftEncoderValue()+
                MyDriveTrain.GetRightEncoderValue())/2) <= 0){
            return(true);
        }
        else{
            return(false);
        }
    }

    protected void end() {
        MyDriveTrain.StopEncoders();
    }

    protected void interrupted() {
    }
}